/** \example orshowsensors.cpp
    \author Rosen Diankov

    Shows how to toggle sensor power and rendering options

    \image html showsensors_camera.jpg "Camera Sensor."
    \image latex showsensors_camera.jpg "Camera Sensor." width=20cm

    \image html showsensors_laser.jpg "Laser Sensor."
    \image latex showsensors_laser.jpg "Laser Sensor." width=20cm

    <b>Full Example Code:</b>
 */
#include <openrave-core.h>
#include <vector>
#include <iostream>

#include "orexample.h"

using namespace OpenRAVE;
using namespace std;

namespace cppexamples {

class ShowSensorsExample : public OpenRAVEExample
{
public:
    virtual void demothread(int argc, char ** argv) {
/*
        penv->Load("data/testwamcamera.env.xml");
        size_t ienablesensor = 0;
        // get all the sensors, this includes all attached robot sensors
        std::vector<SensorBasePtr> sensors;
        penv->GetSensors(sensors);
        while(IsOk()) {
            for(size_t isensor = 0; isensor < sensors.size(); ++isensor) {
                sensors[isensor]->Configure(isensor == ienablesensor ? SensorBase::CC_PowerOn : SensorBase::CC_PowerOff);
                sensors[isensor]->Configure(isensor == ienablesensor ? SensorBase::CC_RenderDataOn : SensorBase::CC_RenderDataOff);
            }
            ienablesensor = (ienablesensor+1)%sensors.size();
            boost::this_thread::sleep(boost::posix_time::seconds(5));
        }
*/
		penv->Load("test.env.xml");

        // attach a physics engine
        penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode"));
        penv->GetPhysicsEngine()->SetGravity(Vector(0,0,-9.8));

		// Find all the robots and get a pointer to them
        vector<RobotBasePtr> vrobots;
        penv->GetRobots(vrobots);
        RobotBasePtr probot = vrobots.at(0);




        vector<int> jointIndex;
        ControllerBasePtr jointController;


		{

			EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
/*
            MultiControllerPtr multi(new MultiController(penv));
            vector<int> dofindices(probot->GetDOF());
            for(int i = 0; i < probot->GetDOF(); ++i) {
                dofindices[i] = i;
            }
            probot->SetController(multi,dofindices,1); // control everything
*/

jointController = RaveCreateController(penv,"odevelocity");
            vector<int> dofindices(probot->GetDOF());
            for(int i = 0; i < probot->GetDOF(); ++i) {
                dofindices[i] = i;
            }
			probot->SetController(jointController, dofindices,1) ;



            // set the velocity controller on all joints that have 'wheel' in their description
            for(std::vector<KinBody::JointPtr>::const_iterator itjoint = probot->GetJoints().begin(); itjoint != probot->GetJoints().end(); ++itjoint) {
				cout << "Got joint: " << (*itjoint)->GetName() << " - ";
	                if( (*itjoint)->GetName().find("j") != string::npos ) {
		                for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
							std::cout << "DOFIndex: " << (*itjoint)->GetDOFIndex()+i << std::endl;
							jointIndex.push_back((*itjoint)->GetDOFIndex()+i);
		                }
                	}			
            }

			// Attach controllers to the joints
			std::cout << "Attaching controllers.\n";
			std::cout << "jointIndex size: " << jointIndex.size() << std::endl;
 /*           if(jointIndex.size() > 0 ) {
				jointController = RaveCreateController(penv,"odevelocity");
				multi->AttachController(jointController, jointIndex, 0);
            }
*/


		}

			std::vector<dReal> tau;
			std::vector<dReal> q;
			std::vector<dReal> q_dot;
			std::vector<dReal> q_d;
			std::vector<dReal> q_dot_d;
			tau.resize(3);
			q.resize(3);
			q_dot.resize(3);
			q_d.resize(3);
			q_dot_d.resize(3);


//			q_d[0] = -0.7;
//			q_d[1] = 2.4;
//			q_d[2] = -2.6;
//			q_d[0] = -45*M_PI/180;
//			q_d[1] = 90*M_PI/180;
//			q_d[2] = -90*M_PI/180;

			q_d[0] = -70*M_PI/180;
			q_d[1] = 90*M_PI/180;
			q_d[2] = -110*M_PI/180;


			q_dot_d[0] = 0;
			q_dot_d[1] = 0;
			q_dot_d[2] = 0;

		std::cout << "Entering loop\n";

dReal pTerm;
dReal dTerm;
dReal iTerm[3];


		float old_time;

		double intError;

		std::cout << "Aiming for positions: " << q_d[0] << ":" << q_d[1] << ":" << q_d[2] << "\n";

		std::cout << "SimTime:Joint0:Pos:vel:torque:Joint1:Pos:vel:torque:Joint2:Pos:vel:torque\n";

		while(IsOk()) {

			EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
			
                if( !!jointController ) {
/*
                   stringstream sout,ss; ss << "setvelocity ";
                    for(size_t i = 0; i < jointIndex.size(); ++i) {
                        //ss << 2*(RaveRandomFloat()-0.5) << " ";
						ss << 1 << " ";
                    }
                    if( !jointController->SendCommand(sout,ss) ) {
                        RAVELOG_WARN("failed to send velocity command\n");
                    }
*/



			// Get Joint position and velocities
			probot->GetDOFValues(q);
			probot->GetDOFVelocities(q_dot);

//std::cout << "Joint: 0 pos: " << q[0] << std::endl;
//std::cout << "Joint: 0 vel: " << q_dot[0] << std::endl;



// torque = -Kv(vel - velDes) - Kp(pos - posDes)

	       	float simtime = penv->GetSimulationTime()*1e-6;
			if ((simtime - old_time) > 0) {
        		//std::cout << "sim time: " << simtime << ": ";
				std::cout << simtime;
			}	



			dReal Kp = 100.0;
			dReal Kv = 60.0;		//2*sqrt(Kp);
			dReal Ki = 0.02;
			for (int i=0; i<3; i++) {

				pTerm = Kp*(q[i] - q_d[i]);
				dTerm = Kv*(q_dot[i] - q_dot_d[i]);
				iTerm[i] += (q[i] - q_d[i]) * Ki;

				tau[i] = -dTerm - pTerm - iTerm[i];

//				tau[i] = -Kv * (q_dot[i] - q_dot_d[i]) - Kp*(q[i] - q_d[i]);
				if ((simtime - old_time) > 0) {
					//std::cout << "Joint:" << i << ":Pos:" << q[i] << ":Vel:" << q_dot[i] << ":Torque:" << tau[i] << ":";	//<< std::endl;
					std::cout << ":" <<  i << ":" << q[i] << ":" << q_dot[i] << ":" << tau[i];
				}

			}
			if ((simtime - old_time) > 0) {
				std::cout << std::endl;
			}


			old_time = simtime;

			// Set joint torques
			probot->SetDOFTorques(tau, false);




                }


		}


    }
};

} // end namespace cppexamples

int main(int argc, char ** argv)
{
    cppexamples::ShowSensorsExample example;
    return example.main(argc,argv);
}
